/*
This file is part of CanFestival, a library implementing CanOpen Stack.

Copyright (C): Edouard TISSERANT and Francis DUPIN

See COPYING file for copyrights details.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stddef.h>		/* for NULL */
#include <errno.h>


#include <unistd.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include "linux/can.h"
#include "linux/can/raw.h"
#include "net/if.h"
#ifndef PF_CAN
#define PF_CAN 29
#endif

#ifndef AF_CAN
#define AF_CAN PF_CAN
#endif


#define CAN_IFNAME     "can%s"


#include "can_driver.h"



static CAN_HANDLE global_fd0=NULL;


extern NodeInfor nodeinfor[NODE_NUM];


/*********functions which permit to communicate with the board****************/
UNS8  canReceive_driver (CAN_HANDLE fd0, Message * m)
{
  int res = 0;
  struct can_frame frame;
  int cnt=0;
  char *ptr=NULL;

  unsigned char frame_len = sizeof(frame);

  while (res <= 0) {
    //res = recv (*(int *) fd0, &frame, sizeof (frame), 0);
	res = recv (*(int *) fd0, &frame, frame_len, 0);
	printf("%s() res:%d  frame_size:%d\n",__func__,res,sizeof(frame));
    if (res < 0)
    {
        if (errno == EBADF)
        {
            /* this will exit recieve loop */
            return 1;
        }
        else if (errno == EINTR) continue; /* retry immediatly */
        else
        {
            /* print error and retry after 20ms, arbitrary */
            fprintf (stderr, "Recv failed: %s\n", strerror (errno));
            usleep(20000);
            continue;
        }
    }
    else if (res == 0)
    {
        /* this will exit recieve loop */
        return 1;
    }
	
	else if(res == frame_len)
	{
		ptr = (char*)&frame;
		printf(".....start.....\n");
		for(cnt=0;cnt<frame_len;cnt++)
		{
			printf("[%d]:0x%x ",cnt,*(ptr+cnt));
		}
		printf("\n");
		printf(".....end.....\n");
		break;
	}
	
  } 

 // m->cob_id = frame.can_id & CAN_EFF_MASK;
 // m->cob_id = frame.can_id&CAN_SFF_MASK;
  ptr = (char*)&frame;
  m->cob_id = *(ptr+0)|(*(ptr+1)<<8)|(*(ptr+2)<<16)|(*(ptr+3)<<24);
  m->cob_id = m->cob_id&CAN_SFF_MASK;
  printf("%s()..... id:0x%x\n",__func__,m->cob_id);
//  m->len = frame.can_dlc;
  m->len = *(ptr+4);

  //if (frame.can_id & CAN_RTR_FLAG)
  if (m->cob_id & CAN_RTR_FLAG)	
    m->rtr = 1;
  else
    m->rtr = 0;

 // memcpy (m->data, frame.data, 8);
  memcpy (m->data, ptr+8, 8);
  ptr = NULL; 

  return 0;
}


/***************************************************************************/
UNS8 canSend_driver (CAN_HANDLE fd0, Message const * m)
{
  int res;
  struct can_frame frame;

  frame.can_id = m->cob_id;
  if (frame.can_id >= 0x800)
    frame.can_id |= CAN_EFF_FLAG;
  frame.can_dlc = m->len;
  if (m->rtr)
    frame.can_id |= CAN_RTR_FLAG;
  else
    memcpy (frame.data, m->data, 8);

#if defined DEBUG_MSG_CONSOLE_ON
  MSG("out : ");
  print_message(m);
#endif
  res = send (*(int *) fd0, &frame, sizeof (frame), 0);
  if (res <= 0)
    {
      fprintf (stderr, "Send failed: %s\n", strerror (errno));
      return 1;
    }


	//add some delay
	usleep(10*1000);

  return 0;
}



/***************************************************************************/
CAN_HANDLE canOpen_driver (void)
{
	struct ifreq ifr;
	struct sockaddr_can addr;
	int err;

	/* fd0 won't be released when leaving the function */
	CAN_HANDLE fd0 = malloc (sizeof (int));
	if(!fd0)
	{
		return NULL;
	}
	
	/* use CAN_RAW protocol and creat the socket */
	*(int *) fd0 = socket (PF_CAN, SOCK_RAW, CAN_RAW);
	if (*(int *) fd0 < 0)
	{
		fprintf (stderr, "Socket creation failed: %s\n",
		strerror (errno));
		goto error_ret;
	}

	/* can0 is used */
	snprintf (ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, "0");
	err = ioctl (*(int *) fd0, SIOCGIFINDEX, &ifr);
	if (err)
	{
		fprintf (stderr, "Getting IF index for %s failed: %s\n",
		ifr.ifr_name, strerror (errno));
		goto error_close;
	}
 
#if 0
    int loopback = 1;
    err = setsockopt(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_LOOPBACK,
               &loopback, sizeof(loopback));
    if (err) 
	{
        fprintf(stderr, "setsockopt: %s\n", strerror (errno));
        goto error_close;
    }  

  
    int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */
    err = setsockopt(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
               &recv_own_msgs, sizeof(recv_own_msgs));
    if (err) {
        fprintf(stderr, "setsockopt: %s\n", strerror (errno));
        goto error_close;
    }
#endif 

    /* bind can0 */
	addr.can_family = AF_CAN;
	addr.can_ifindex = ifr.ifr_ifindex;
	err = bind (*(int *) fd0, (struct sockaddr *) &addr, sizeof (addr));
	if (err)
	{
		fprintf (stderr, "Binding failed: %s\n", strerror (errno));
		goto error_close;
	}


  global_fd0 = fd0;
  return fd0;
error_close:
	close (*(int *) fd0);
	global_fd0 = NULL;

error_ret:
  free (fd0);
  global_fd0 = NULL;
  return NULL;
}

/***************************************************************************/
int canClose_driver (CAN_HANDLE fd0)
{
	if (fd0)
	{
		shutdown (*(int *) fd0, SHUT_RDWR);
		close (*(int *) fd0);
		free (fd0);
	}

	global_fd0 = NULL;
	return 0;
}




int canInit_driver(void)
{
	/* disable can0 interface */
	system("ifconfig can0 down");

	/* set can0 baudrate */
	system("ip link set can0 type can bitrate 125000");

	/* enable can0 */
	system("ip link set can0 up");

	return 0;
}


/*
when can node power up,it will run
	boot-up(0x05)---->pre-operational(0x7F)--->operationals(0x05)
*/
int canMotor_EnterOperational(CAN_HANDLE fd0,int id)
{
	Message msg;

	msg.cob_id = 0;
	//make can node enter operational state
	msg.data[0]= 0x01;
	msg.data[1]= id;
	msg.len = 2;
	msg.rtr = 0;

	canSend_driver(fd0,&msg);

	return 0;
}






 int canMotor_Enable(CAN_HANDLE fd0,int id)
{
	Message msg;

	msg.cob_id = 0x600 +id;
	//2B 40 60 00 06 00 00 00,ready to switch on
	msg.data[0]= 0x2B;
	msg.data[1]= 0x40;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= 0x06;
	msg.data[5]= 0x00;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);

	//2B 40 60 00 07 00 00 00,switched on
	msg.data[4]= 0x07;
	canSend_driver(fd0,&msg);

	//2B 40 60 00 0F 00 00 00,operation enabled
	msg.data[4]= 0x0F;
	canSend_driver(fd0,&msg);	
	return 0;
}



int canMotor_SetPositionParameter(CAN_HANDLE fd0,int id,PositionParam_t*param)
{
	Message msg;
	UNS16 velocity,acceleration,deceleration;
	UNS32 steps = 0;

	msg.cob_id = 0x600 +id;
	
	//2F 60 60 00 01 00 00 00 set position mode
	msg.data[0]= 0x2F;
	msg.data[1]= 0x60;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= 0x01;
	msg.data[5]= 0x00;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);	

	if(param->dirflag == CW)
	{
		steps = param->targetposition;	
		printf("CW run\n");
	}
	else if(param->dirflag == CCW)
	{
		steps = ~param->targetposition;
		steps += 1;
		printf("CCW run steps:0x%x\n",steps);
	}
	else
	{
		steps = 0;
	}
	
	//23 81 60 00 E0 01 00 00 set velocity 
	velocity = param->velocity*240;
	msg.data[0]= 0x23;
	msg.data[1]= 0x81;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= velocity&0xff;
	msg.data[5]= (velocity>>8)&0xff;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;	
	canSend_driver(fd0,&msg);


	acceleration = param->acceleration*6;
	//23 83 60 00 58 02 00 00 set acceleration 
	msg.data[0]= 0x23;
	msg.data[1]= 0x83;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= acceleration&0xff;
	msg.data[5]= (acceleration>>8)&0xff;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);


	
	deceleration = param->deceleration*6;
	//23 84 60 00 58 02 00 00 set deceleration 
	msg.data[0]= 0x23;
	msg.data[1]= 0x84;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= deceleration&0xff;
	msg.data[5]= (deceleration>>8)&0xff;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);


	//23 7A 60 00 xx xx xx xx set target position
	msg.data[0]= 0x23;
	msg.data[1]= 0x7A;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
#if 0
	msg.data[4]= param->targetposition&0xff;
	msg.data[5]= (param->targetposition>>8)&0xff;
	msg.data[6]= (param->targetposition>>16)&0xff;
	msg.data[7]= (param->targetposition>>24)&0xff;
#else
	msg.data[4]= steps&0xff;
	msg.data[5]= (steps>>8)&0xff;
	msg.data[6]= (steps>>16)&0xff;
	msg.data[7]= (steps>>24)&0xff;
	printf("byte[4]:0x%02x\n",msg.data[4]);
	printf("byte[5]:0x%02x\n",msg.data[5]);
	printf("byte[6]:0x%02x\n",msg.data[6]);
	printf("byte[7]:0x%02x\n",msg.data[7]);
#endif
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);

}



int canMotor_ExecuteRelative(CAN_HANDLE fd0,int id)
{
	Message msg;
	int status;
	
	msg.cob_id = 0x600 +id;

	//2B 40 60 00 5F 00 00 00,set new set point bit to 1s
	msg.data[0]= 0x2B;
	msg.data[1]= 0x40;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= 0x5F;
	msg.data[5]= 0x00;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);

	//wait until the bit12 ack high
	
	do
	{
		canMotor_GetStatus(fd0,0x01,&status);
		usleep(10*1000);
	}while(status&(0x1<<12) == 0);
	
	//2B 40 60 00 4F 00 00 00,clear new set point bit
	msg.data[0]= 0x2B;
	msg.data[1]= 0x40;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= 0x4F;
	msg.data[5]= 0x00;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);

	return 0;
	
}



int canMotor_GetStatus(CAN_HANDLE fd0,int id,unsigned int *status)
{
	Message msg;
	Message msg_rx;
	int cnt=0;
	msg.cob_id = 0x600 +id;

	

	//2B 40 60 00 5F 00 00 00,set new set point bit to 1s
	msg.data[0]= 0x40;
	msg.data[1]= 0x41;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= 0x00;
	msg.data[5]= 0x00;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);
#if 0
	canReceive_driver(fd0,&msg_rx);

	if((msg_rx.cob_id == 0x281)||(msg_rx.cob_id ==0x381)||(msg_rx.cob_id ==0x481))
		return -1;

	

	//if((msg_rx.cob_id == 0x581)&&(msg_rx.data[1]==0x41)&&(msg_rx.data[2]==0x60))
	if(msg_rx.cob_id == 0x581)	
	{
		printf("%s() rcv start\n",__func__);
		for(cnt=0;cnt<msg_rx.len;cnt++)
		{
			printf("data[%d]:0x%02x ",cnt,msg_rx.data[cnt]);		
		}
		printf("%s() rcv end\n",__func__);
		
		*status = (msg_rx.data[5]<<8)|msg_rx.data[4];
		printf("status:0x%x\n",*status);

	}
	else if(msg.cob_id == (0x700+id))
	{
		*status = (msg_rx.data[5]<<8)|msg_rx.data[4];
	}
#endif		

	return 0;
}



int canMotor_QueryStatus(CAN_HANDLE fd0,int id)
{
	Message msg;
	//Message msg_rx;
	//int cnt=0;
	msg.cob_id = 0x600 +id;

	

	//2B 40 60 00 5F 00 00 00,set new set point bit to 1s
	msg.data[0]= 0x40;
	msg.data[1]= 0x41;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= 0x00;
	msg.data[5]= 0x00;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);
#if 0
	canReceive_driver(fd0,&msg_rx);

	if((msg_rx.cob_id == 0x281)||(msg_rx.cob_id ==0x381)||(msg_rx.cob_id ==0x481))
		return -1;

	

	//if((msg_rx.cob_id == 0x581)&&(msg_rx.data[1]==0x41)&&(msg_rx.data[2]==0x60))
	if(msg_rx.cob_id == 0x581)	
	{
		printf("%s() rcv start\n",__func__);
		for(cnt=0;cnt<msg_rx.len;cnt++)
		{
			printf("data[%d]:0x%02x ",cnt,msg_rx.data[cnt]);		
		}
		printf("%s() rcv end\n",__func__);
		
		*status = (msg_rx.data[5]<<8)|msg_rx.data[4];
		printf("status:0x%x\n",*status);

	}
	else if(msg.cob_id == (0x700+id))
	{
		*status = (msg_rx.data[5]<<8)|msg_rx.data[4];
	}
#endif		

	return 0;
}


int canMotor_Test(CAN_HANDLE fd0,int id)
{
	canMotor_Enable(fd0,id);
	
	PositionParam_t param;
	param.acceleration=10;//100;//100rps/s
	param.deceleration=10;//100;//100rps/s
	param.velocity=2;
	param.dirflag=CW;
	param.targetposition=200; // 200 steps because the motor is configued as 200steps/rev

	canMotor_SetPositionParameter(fd0,id,&param);
	canMotor_ExecuteRelative(fd0,id);

	return 0;
}


int canMotor_Test_CCW(CAN_HANDLE fd0,int id)
{
	canMotor_Enable(fd0,id);
	
	PositionParam_t param;
	param.acceleration=10;//100rps/s
	param.deceleration=10;//100rps/s
	param.velocity=2;
	param.dirflag = CCW;
	param.targetposition=200; // 200 steps because the motor is configued as 200steps/rev

	canMotor_SetPositionParameter(fd0,id,&param);
	canMotor_ExecuteRelative(fd0,id);

	return 0;
}



int canMotor_Parse(Message *pmsg)
{
	UNS16 cob_id=0;
	UNS16 node_id=0;

	int cnt=0;
	
	if(pmsg == NULL)
		return -1;
	
	cob_id = pmsg->cob_id&0xFF00;
	node_id = pmsg->cob_id &0x00FF;

	//NMT process
	if(cob_id == 0x700)
	{
		// the node is in pre-operational state
		if(pmsg->data[0] == 0x7F)
		{
			canMotor_EnterOperational(global_fd0,node_id);

			
			printf("enable node:0x%x\n",node_id);

			
			for(cnt=0;cnt<pmsg->len;cnt++)
			{
				printf("id:0x%x data[%d]:0x%x ",node_id,cnt,pmsg->data[cnt]);
			}
			printf("\n");
			
			memset(pmsg,0,sizeof(Message));
		}
		else if(pmsg->data[0] == 0x05)
		{
			for(cnt=0;cnt<NODE_NUM;cnt++)
			{
				if(nodeinfor[cnt].node_id == node_id)
				{
					nodeinfor[cnt].state = NODE_VALID;
					printf("node:%d is valid\n",node_id);
					break;
				}
				/*
				else
				{
					nodeinfor[cnt].state = NODE_INVALID;	
					printf("node:%d is in valid\n",node_id);
					break;
				}
				*/
			}

		}
	}
	

}



static int canMotor_HomeMode(CAN_HANDLE fd0,int id)
{
	Message msg;
	msg.cob_id = 0x600 +id;
	
	//2F 60 60 00 01 00 00 00 set home mode
	msg.data[0]= 0x2F;
	msg.data[1]= 0x60;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= 0x06;
	msg.data[5]= 0x00;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);
	return 0;
}





static int canMotor_SetHomeParameter(CAN_HANDLE fd0,int id,PositionParam_t*param)
{
	Message msg;
	UNS16 velocity,acceleration,offset,index;
	UNS32 steps = 0;

	msg.cob_id = 0x600 +id;



	//set home method
	msg.data[0]= 0x2F;
	msg.data[1]= 0x98;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= param->home_method;
	msg.data[5]= 0x00;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);	


	//set acceleration 
	acceleration = param->home_acceleration*6;	
	msg.data[0]= 0x23;
	msg.data[1]= 0x9A;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= acceleration&0xff;
	msg.data[5]= (acceleration>>8)&0xff;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);

	
	//set velocity 
	velocity = param->home_velocity*240;
	msg.data[0]= 0x23;
	msg.data[1]= 0x99;
	msg.data[2]= 0x60;
	msg.data[3]= 0x01;
	msg.data[4]= velocity&0xff;
	msg.data[5]= (velocity>>8)&0xff;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;	
	canSend_driver(fd0,&msg);


	//set index velocity to 0.5rps
	msg.data[0]= 0x23;
	msg.data[1]= 0x99;
	msg.data[2]= 0x60;
	msg.data[3]= 0x02;
	msg.data[4]= 0x78;
	msg.data[5]= 0x00;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);


	//set home offset
	
	offset = param->home_offset;
	msg.data[0]= 0x23;
	msg.data[1]= 0x7C;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= offset&0xff;
	msg.data[5]= (offset>>8)&0xff;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	//canSend_driver(fd0,&msg);

	//set home switch	
	msg.data[0]= 0x2F;
	msg.data[1]= 0x01;
	msg.data[2]= 0x70;
	msg.data[3]= 0x00;
	msg.data[4]= param->home_switch&0xff;
	msg.data[5]= 0x00;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);	

	return 0;
}



static int canMotor_HomeStart(CAN_HANDLE fd0,int id)
{
	Message msg;
	
	msg.cob_id = 0x600 +id;


	//transition bit4 of data[4]from 0 to 1, to start home
	msg.data[0]= 0x2B;
	msg.data[1]= 0x40;
	msg.data[2]= 0x60;
	msg.data[3]= 0x00;
	msg.data[4]= 0x0F;
	msg.data[5]= 0x00;
	msg.data[6]= 0x00;
	msg.data[7]= 0x00;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);

	msg.data[4]= 0x1F;
	canSend_driver(fd0,&msg);


	return 0;

}



static int canMotor_HomeGetStatus(CAN_HANDLE fd0,int id)
{
	unsigned int status;
	unsigned int tmp=0;
	int ret;

	ret = canMotor_GetStatus(fd0,id,&status);
#if 0	
	if(ret < 0)
		return -3;
	
	if(status&(0x1<<13))
	{
		printf("homing error!\n");
		return -1;
	}

	tmp = status&(0x1<<12);
	//if(status&(0x1<<12) == 0)
	printf("tmp:0x%x\n",tmp);
	if(tmp == 0)
	{
		printf("home mode not yet complete!\n");
		return -2;
	}
	else
	{
		printf("home mode carried out successfully\n");
	}
#endif	

	return 0;

}


int canMotor_Home(CAN_HANDLE fd0,int id,PositionParam_t*param)
{
	int ret=0;
	/*
	ret = canMotor_HomeGetStatus(fd0,id);
	if(ret < 0)
		return -1;
	*/
	
	printf("motor homing....\n");
	canMotor_Enable(fd0,id);

	canMotor_HomeMode(fd0,id);

	canMotor_SetHomeParameter(fd0,id,param);
	
	canMotor_HomeStart(fd0,id);

	return 0;
}



int canMotor_HomeModeTest(CAN_HANDLE fd0,int id)
{
	PositionParam_t homeParam;
	
	homeParam.home_acceleration = 100;
	homeParam.home_velocity = 2;
	homeParam.home_offset = 40000;
	homeParam.home_switch=3;
	homeParam.home_method = 17;
	
	canMotor_Home(fd0,id,&homeParam);

	return 0;
}



int canMotor_FilterConfig(CAN_HANDLE fd0,int id)
{
		struct can_filter rfilter[4]={0};  

		unsigned int id_1,id_2;
		unsigned int mask;
		id_1 = 0x701;
		id_2 = 0x581;
		mask = id_1&id_2;
		mask |= CAN_EFF_FLAG | CAN_RTR_FLAG;
		
		rfilter[0].can_id	= 0x701;  
		//rfilter[0].can_mask = CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_SFF_MASK;//CAN_EFF_FLAG|CAN_SFF_MASK; 
		rfilter[0].can_mask = mask;//CAN_EFF_FLAG | CAN_RTR_FLAG | 0x501;//CAN_EFF_FLAG|CAN_SFF_MASK;
		
		rfilter[1].can_id	= 0x581;  
		rfilter[1].can_mask = mask;//CAN_EFF_FLAG | CAN_RTR_FLAG | 0x501;; 
		
		//rfilter[2].can_id	= 0x281;  
		//rfilter[2].can_mask = CAN_EFF_FLAG|~0x281;
		
		//rfilter[3].can_id	= 0x281;  
		//rfilter[3].can_mask = CAN_SFF_MASK; 	
		//setsockopt(*(int*)fd0, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter[0], sizeof(struct can_filter));
		//setsockopt(*(int*)fd0, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter[1], sizeof(struct can_filter));
		setsockopt(*(int*)fd0, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter));


	return 0;

}
int canMotor_Reset(CAN_HANDLE fd0,int id)
{
	Message msg;

	msg.cob_id = 0x000;
	//2B 40 60 00 06 00 00 00,ready to switch on
	msg.data[0]= 0x81;
	msg.data[1]= id;
	msg.len = 2;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);
	return 0;
}

int canMotor_DisbalePDOs(CAN_HANDLE fd0,int id)
{
	Message msg;
	Message msg_rx;
	msg.cob_id = 0x600 +id;

	//disable PDO 0x180+ID
	msg.data[0]= 0x23;
	msg.data[1]= 0x00;
	msg.data[2]= 0x18;
	msg.data[3]= 0x01;
	msg.data[4]= 0x80+id;
	msg.data[5]= 0x01;
	msg.data[6]= 0x00;
	msg.data[7]= 0x80;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);


	//disable PDO 0x280+ID
	msg.data[0]= 0x23;
	msg.data[1]= 0x01;
	msg.data[2]= 0x18;
	msg.data[3]= 0x01;
	msg.data[4]= 0x80+id;
	msg.data[5]= 0x02;
	msg.data[6]= 0x00;
	msg.data[7]= 0x80;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);

	//disable PDO 0x380+ID
	msg.data[0]= 0x23;
	msg.data[1]= 0x02;
	msg.data[2]= 0x18;
	msg.data[3]= 0x01;
	msg.data[4]= 0x80+id;
	msg.data[5]= 0x03;
	msg.data[6]= 0x00;
	msg.data[7]= 0x80;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);

	//disable PDO 0x380+ID
	msg.data[0]= 0x23;
	msg.data[1]= 0x03;
	msg.data[2]= 0x18;
	msg.data[3]= 0x01;
	msg.data[4]= 0x80+id;
	msg.data[5]= 0x04;
	msg.data[6]= 0x00;
	msg.data[7]= 0x80;
	msg.len = 8;
	msg.rtr = 0;
	canSend_driver(fd0,&msg);

}


